package org.sunspotworld;

import com.sun.spot.peripheral.Spot;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.ISwitch;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import java.io.IOException;
import com.iaroc.irobot.util.KeyedStorage;

/**
 *
 * @author Administrator
 */
public class SpotBot {
    // constants
    public static int CRUISESPEED           = 60;
    public static int TURNSPEED             = 40;
    public static final int TOOCLOSE        = 18;
    public static final int CLOSE        = 8;
    public static final int FAR        = 17;
    
    public static final int SEEKING_WALL    = 20;
    public static final int FOLLOWING_WALL  = 20;
    public static final int TURNING_RIGHT   = 20;
    public static final int TURNING_LEFT    = 20;
    public static final int LEFT_FRONT      = 0;
    public static final int RIGHT_FRONT     = 1;
    public static final int LEFT_REAR       = 2;
    public static final int RIGHT_REAR      = 3;
    public static final boolean DEBUG = true;
    
    public static final int ROTATE_90   = 17;
    public static final int ROTATE_180  = 34;
    public static final int TURN_90     = 34;
    public static final int TURN_180    = 68;
    public static final int TURN_45     = 17;
    
    int lastTurn = MotorController.TURN_LEFT;
    
    private BeaconSensor beaconSensor;
    private IrSensor irSensors;
    private SonarSensor sonarSensor;
    private SonarServo sonarServo;
    private SensorArray odometrySensors = new SensorArray();
    private MotorController motors = new MotorController();
    private float frontIRat5 = 0;
    private float frontIRat10 = 0;
    private float frontIRat15 = 0;
    private float frontIRat20 = 0;
    private float rearIRat5 = 0;
    private float rearIRat10 = 0;
    private float rearIRat15 = 0;
    private float rearIRat20 = 0;
    EDemoBoard demo = EDemoBoard.getInstance();
    ISwitch sw1 = demo.getSwitches()[0]; 
    ISwitch sw2 = demo.getSwitches()[1]; 
    ITriColorLED[] leds = demo.getLEDs();
    private KeyedStorage sensorOffsets = new KeyedStorage("sensorOffsets");
    private KeyedStorage mazeMap = new KeyedStorage("mazeMap");
    private Maze maze = new Maze();
    
    float leftFront, leftRear, rightFront, rightRear, front;
    
    // initialize the spotbot here
    // D0 is rx D1 is tx for UART
    public SpotBot(){
        Spot.getInstance().getSleepManager().disableDeepSleep();
        Spot.getInstance().getSleepManager().disableDiagnosticMode();
        
        // set LED color
        for( int i = 0; i < 8; i++){
            leds[i].setRGB( 25, 0, 50);
        }
        
        
        // initialize the UART
        demo.initUART(38400, false);  // baud_rate, two_stop_bits
        
        //flush receive buffer.
//         try
//         {
//            while(true)
//               demo.readUART();
//         }
//         catch(IOException ioe) {} 
        
        sonarSensor = new SonarSensor(EDemoBoard.D3);
        irSensors = new IrSensor();
        //sonarServo = new SonarServo(EDemoBoard.D2);
        beaconSensor = new BeaconSensor(EDemoBoard.D4);

        irSensors.start();
        
        toSleep(500);
        //sonarSensor.start();
        //toSleep(500);
        //sonarServo.start();
        motors.sendCommand( MotorController.SET_TICK_SPEED, 120 );
        run();
    }
    
    // main logic loop
    public void run(){
        //this.alignWithWall();
        //motors.sendCommand(MotorController.VOODOO, CRUISESPEED);
        if(sw2.isClosed()){
            this.leds[7].setRGB(50, 0, 0);
            this.leds[7].setOn();
            this.callibrateIR();
//            this.autoCalibrateIR();
            while(sw2.isClosed());
            
            
        }else{
            if( sensorOffsets.getString("frontIRat5") != null ){
                debug("Reading from KeyedStore2");
                // load in the values from the record store if they are present
                frontIRat5 = 0;//Float.parseFloat(sensorOffsets.getString("frontIRat5"));
                frontIRat10 = 0;//Float.parseFloat(sensorOffsets.getString("frontIRat10"));
                frontIRat15 = 0;//Float.parseFloat(sensorOffsets.getString("frontIRat15"));
                frontIRat20 = 0;//Float.parseFloat(sensorOffsets.getString("frontIRat20"));

                frontIRat5 = 0;//Float.parseFloat(sensorOffsets.getString("rearIRat5"));
                frontIRat10 = 0;//Float.parseFloat(sensorOffsets.getString("rearIRat10"));
                frontIRat15 = 0;//Float.parseFloat(sensorOffsets.getString("rearIRat15"));
                frontIRat20 = 0;//Float.parseFloat(sensorOffsets.getString("rearIRat20"));
                debug("frontIRat5: " + Float.toString(frontIRat5) + "frontIRat10: " + Float.toString(frontIRat10) + "frontIRat15: " + Float.toString(frontIRat15) + "frontIRat20: " + Float.toString(frontIRat20) + "rearIRat5: " + Float.toString(rearIRat5) + "rearIRat10: " + Float.toString(rearIRat10) + "rearIRat15: " + Float.toString(rearIRat15) + "rearIRat20: " + Float.toString(rearIRat20) );
            }
        }
        while(sw1.isOpen()){
            toSleep(80);
            //this.alignWithWall();
            //followLeftNew();
            //this.followLeft();
            //test();
            //this.findIr();
            this.mazeExplorer();
           

        }   

        //sonarSensor.interrupt(); // exit the SonarSensor thread
    }
    
    // and empty function to run arbitrarily tests
    private void test(){
        //System.out.println("reading " + irSensors.getReading(LEFT_REAR));
        //testSonarReading();
        //this.testIr();
        //this.setLedDifference( irSensors.getReading( LEFT_FRONT ) - irSensors.getReading( LEFT_REAR) );
    }
    
    private void debug( String message ){
        if( DEBUG )
            System.out.println( message );
    }
    
    private long lastTurnTime = System.currentTimeMillis();
    private long lastRotateTime = System.currentTimeMillis();
    private void followLeft(){
        updateIR();
        front = sonarSensor.getReading();
        debug("leftFront: " + leftFront + "leftRear: " + leftRear + "Sonar: " + front);

        if( front <= TOOCLOSE ){
            if( lastRotateTime < System.currentTimeMillis() - 2000){
                debug( "Front too close turning right");
                motors.sendCommand(MotorController.ROTATE_RIGHT_N, 19);
                motors.sendCommand(MotorController.STRAIGHT, CRUISESPEED);
                lastRotateTime = System.currentTimeMillis();
            }
            return;
        } 
      
        
        if( leftFront <= CLOSE ){
            debug("Too close to wall turning away from wall");
            if( lastTurnTime < System.currentTimeMillis() - 500){
                motors.adjustRight();
                motors.adjustRight();
                lastTurnTime = System.currentTimeMillis();
            }
            //turnAwayFromWall( leftFront, leftRear );
            //alignWithWall();        
            return;
        }
        
        if( leftFront >= FAR ){
            if( leftFront - leftRear > 14 && front > TOOCLOSE ){
                if( lastRotateTime < System.currentTimeMillis() - 2000){
                    debug("The front ir dropped off so I'm trying to turn around a left corner");
                    motors.sendCommand(MotorController.FORWARD_N, 12);

                    motors.sendCommand(MotorController.TURN_LEFT_N, 34);
                    motors.sendCommand(MotorController.FORWARD_N, 14);
                    motors.sendCommand(MotorController.STRAIGHT, CRUISESPEED);
                    lastRotateTime = System.currentTimeMillis();
                }    
                
                return;
            }
            debug("Too far from wall turning towards wall");
            //turnTowardWall( leftFront, leftRear );
            if( lastTurnTime < System.currentTimeMillis() - 500){
                motors.adjustLeft();
                motors.adjustLeft();
                lastTurnTime = System.currentTimeMillis();
            }
            //alignWithWall();
            return;
        }
        
        /*
        if(  leftFront > FAR - 3){
            // leftFront is out too far
            if( lastTurnTime < System.currentTimeMillis() - 500){
                motors.adjustLeft();
                lastTurnTime = System.currentTimeMillis();
            }
        } else if(  leftFront < CLOSE + 3 ){
            // leftRear is out too far
            if( lastTurnTime < System.currentTimeMillis() - 500){
                motors.adjustRight();
                lastTurnTime = System.currentTimeMillis();
            }
        } 
         */
            
    }
    
    private float lastLeft = 0;
    private void mazeExplorer(){
        this.updateIR();
        float fullFrontal = sonarSensor.getReading();
        debug("Entering mazeExplorer!");
        debug("leftFront="+leftFront+" leftRear="+leftRear+" fullFrontal="+fullFrontal);
        // there is a wall to the left
        if(leftFront <= 20 && leftRear <= 20)
        {
            if(leftFront <= 6 || leftRear <= 6)
            {
                while(!motors.sendCommand( MotorController.ROTATE_RIGHT_N, this.ROTATE_90));   
                while(!motors.sendCommand( MotorController.STRAIGHT, 6 ));
                while(!motors.sendCommand( MotorController.ROTATE_LEFT_N, this.ROTATE_90));
            }
            debug("Left IR sensors are both sensing a wall.");
            if( fullFrontal >= 70 )
            {
                debug("Sonar does not see a wall within 2 feet of us.");
                debug("We are moving forward 2 feet.");
                //next to a wall and nothing in front so move forward 2'
                this.alignWithWall();
                motors.sendCommand(MotorController.STRAIGHT, 68);
                maze.goForward();
            }
            else if(fullFrontal > 13)
            {
                debug("I is next to a wall so I move forward just a bit");
                // we're next to a wall but there is a wall in front of us 
                // so we'll move up to about 10cm away from it.
                this.alignWithWall();
                fullFrontal = sonarSensor.getReading();
                System.out.println("fullFrontal: " + fullFrontal);
                float numTicks = ((fullFrontal - 12)/(float)0.911);
                motors.sendCommand( MotorController.STRAIGHT, (int)numTicks );
            }
            else
            {
                debug("We are right in front of a wall, rotating 90 degrees CW");
                // if we are here that means we are next to a wall and
                // there is a wall within 13cm or less of us so rotate right
                motors.sendCommand( MotorController.ROTATE_RIGHT_N, ROTATE_90 );
                this.alignWithWall();
                maze.turnRight();
            }
            lastLeft = leftFront;
            return;
        }
        
        if(leftFront >= 20 && leftRear <= 20)
        {
            debug("The left front sensor is picking up an opening but the rear");
            debug("still sees a wall, moving forward a bit");
            motors.sendCommand( MotorController.STRAIGHT, 20);
            
            lastLeft = leftFront;
            return;
        }
        
        if(leftFront <= 20 && leftRear >= 20)
        {
            debug("The left rear sensor is picking up an opening but the front");
            debug("still sees a wall, moving forward a bit");
            motors.sendCommand( MotorController.STRAIGHT, 12);
            
            lastLeft = leftFront;
            return;
        }
        
        if(leftFront >= 20 && leftRear >= 20)
        {
            debug("There is no wall beside us so turn left and move 2 feet");
            while(!motors.sendCommand( MotorController.ROTATE_LEFT_N, ROTATE_90 ));
            maze.turnLeft();
            // 22 ticks is approximately 20 cm
            fullFrontal = sonarSensor.getReading();
            debug("leftFront >= 25");
            // we thought there was no wall, but it ends up we just got away too far
            if(fullFrontal <= 40)
            {
                while(!motors.sendCommand( MotorController.STRAIGHT, (int)fullFrontal - 10));
                while(!motors.sendCommand( MotorController.ROTATE_RIGHT_N, ROTATE_90-2));
                maze.turnRight();
            }
            else
            {
                int ticks = 32;
           
                if(lastLeft > 0)
                    ticks += (((int)lastLeft)*2);
                else
                    ticks += 20;
                debug("ticks: " + ticks);
                while(!motors.sendCommand( MotorController.STRAIGHT, ticks + 2));
                maze.goForward();
            }
            
            lastLeft = leftFront;
            if(lastLeft > 15)
                lastLeft = 0;
            
            return;
        }
    }

    
    private void autoCalibrateIR(){
        while(this.sw2.isClosed());
        front = sonarSensor.getReading();
//        motors.sendCommand( MotorController.SET_TICK_SPEED, 60 );
        if(front >= 30)
        {
            motors.sendCommand( MotorController.ROTATE_LEFT_N, ROTATE_90);
            toSleep(200);
            front = sonarSensor.getReading();
        }
        
        if(front < 8)
        {
            motors.sendCommand( MotorController.REVERSE_N, 8 - (int)front);
        }
        else if(front > 8 )
        {
            motors.sendCommand( MotorController.STRAIGHT, (int)front - 8);
        }
        motors.sendCommand( MotorController.ROTATE_RIGHT_N, ROTATE_90-1 );
        toSleep(200);
        leftFront = irSensors.getReading( LEFT_FRONT );
        leftRear = irSensors.getReading( LEFT_REAR );
        frontIRat5 = 5 - leftFront;
        rearIRat5 = 5 - leftRear;
        this.leds[7].setRGB(50, 50, 0);
        motors.sendCommand( MotorController.ROTATE_RIGHT_N, ROTATE_90);
        motors.sendCommand( MotorController.STRAIGHT, 5);
        motors.sendCommand( MotorController.ROTATE_LEFT_N, ROTATE_90);
        toSleep(200);
        leftFront = irSensors.getReading( LEFT_FRONT );
        leftRear = irSensors.getReading( LEFT_REAR );
        frontIRat10 = 10 - leftFront;
        rearIRat10 = 10 - leftRear;
        this.leds[7].setRGB(0, 50, 0);
        motors.sendCommand( MotorController.ROTATE_RIGHT_N, ROTATE_90-1);
        motors.sendCommand( MotorController.STRAIGHT, 5);
        // fudge factor of 1
        motors.sendCommand( MotorController.ROTATE_LEFT_N, ROTATE_90);
        toSleep(200);
        leftFront = irSensors.getReading( LEFT_FRONT );
        leftRear = irSensors.getReading( LEFT_REAR );
        frontIRat15 = 15 - leftFront;
        rearIRat15 = 15 - leftRear;
        this.leds[7].setRGB(0, 50, 50);
        motors.sendCommand( MotorController.ROTATE_RIGHT_N, ROTATE_90-1);
        motors.sendCommand( MotorController.STRAIGHT, 5);
        motors.sendCommand( MotorController.ROTATE_LEFT_N, ROTATE_90);
        toSleep(200);
        leftFront = irSensors.getReading( LEFT_FRONT );
        leftRear = irSensors.getReading( LEFT_REAR );
        frontIRat20 = 20 - leftFront;
        rearIRat20 = 20 - leftRear;
        this.leds[7].setRGB(0, 0, 50);
        motors.sendCommand( MotorController.ROTATE_LEFT_N, ROTATE_90);
        front = sonarSensor.getReading();
        motors.sendCommand( MotorController.STRAIGHT, (int)front - 10);
        motors.sendCommand( MotorController.ROTATE_RIGHT_N, ROTATE_90);
        this.leds[7].setRGB(50, 50, 50);
//        motors.sendCommand( MotorController.SET_TICK_SPEED, 80 );
        toSleep(500);
        this.leds[7].setOff();
    }
    
    private void niceRotate(int cmd, int ticks){
        if(cmd == MotorController.ROTATE_LEFT_N || cmd == MotorController.ROTATE_RIGHT_N )
        {
            for(int i = 0; i < ticks; i++){
                motors.sendCommand( cmd, 1);
            }
        }
    }
    
    private void callibrateIR(){
        while(this.sw2.isClosed()){
            toSleep(100);
        }
        this.leds[0].setOn();
        while(this.sw2.isOpen()){
            leftFront = irSensors.getReading( LEFT_FRONT );
            leftRear = irSensors.getReading( LEFT_REAR );
            toSleep(100);
           
        }
        System.out.println("LeftFront:"+leftFront);
        this.leds[0].setRGB(0, 50, 0);
        while(this.sw2.isClosed());
        this.leds[1].setOn();
        while(this.sw2.isOpen()){
            leftFront = irSensors.getReading( LEFT_FRONT );
            leftRear = irSensors.getReading( LEFT_REAR );
            toSleep(100);
        }
        frontIRat10 = 10 - leftFront;
        rearIRat10 = 10 - leftRear;
        this.leds[1].setRGB(0, 50, 0);
        while(this.sw2.isClosed());
        this.leds[2].setOn();
        while(this.sw2.isOpen()){
            leftFront = irSensors.getReading( LEFT_FRONT );
            leftRear = irSensors.getReading( LEFT_REAR );
            toSleep(100);
        }
        frontIRat15 = 15 - leftFront;
        rearIRat15 = 15 - leftRear;
        this.leds[2].setRGB(0, 50, 0);
        while(this.sw2.isClosed());
        this.leds[3].setOn();
        while(this.sw2.isOpen()){
            leftFront = irSensors.getReading( LEFT_FRONT );
            leftRear = irSensors.getReading( LEFT_REAR );
            toSleep(100);
        }
        frontIRat20 = 20 - leftFront;
        rearIRat20 = 20 - leftRear;
        this.leds[3].setRGB(0, 50, 0);
        while(this.sw2.isClosed());
        
        // store the offsets in the record store
        sensorOffsets.putString("frontIRat5", Float.toString(frontIRat5));
        sensorOffsets.putString("frontIRat10", Float.toString(frontIRat10));
        sensorOffsets.putString("frontIRat15", Float.toString(frontIRat15));
        sensorOffsets.putString("frontIRat20", Float.toString(frontIRat20));
        
        sensorOffsets.putString("rearIRat5", Float.toString(rearIRat5));
        sensorOffsets.putString("rearIRat10", Float.toString(rearIRat10));
        sensorOffsets.putString("rearIRat15", Float.toString(rearIRat15));
        sensorOffsets.putString("rearIRat20", Float.toString(rearIRat20));
        
        debug( "The values being put in the store are: ");
        System.out.println("LfIr"+frontIRat5);
        debug(Float.toString(frontIRat5));
         debug(Float.toString(frontIRat10));
         debug(Float.toString(frontIRat15));
         debug(Float.toString(frontIRat20));
        
         debug(Float.toString(rearIRat5));
         debug(Float.toString(rearIRat10));
         debug(Float.toString(rearIRat15));
         debug(Float.toString(rearIRat20));

        this.leds[0].setRGB( 0, 50, 0);
        this.leds[1].setRGB( 0, 50, 0);
        this.leds[2].setRGB( 0, 50, 0);
        this.leds[3].setRGB( 0, 50, 0);
    }
    
    private void updateIR(){
        leftFront = irSensors.getReading( LEFT_FRONT );
        System.out.println("UpdateIR:LeftIR="+leftFront);
        if(leftFront >= 2.5 && leftFront < 7.5)
            leftFront += frontIRat5;
        else if(leftFront >= 7.5 && leftFront < 12.5 )
            leftFront += frontIRat10;
        else if(leftFront >= 12.5 && leftFront < 17.5)
            leftFront += frontIRat15;
        else
            leftFront += frontIRat20;
        // ugly hack to equalize sensors, just add 0.6 to the rear!  YUCK!!!
        leftRear = irSensors.getReading( LEFT_REAR );
        if(leftRear >= 2.5 && leftRear < 7.5)
            leftRear += rearIRat5;
        else if(leftRear >= 7.5 && leftRear < 12.5)
            leftRear += rearIRat10;
        else if(leftRear >= 12.5 && leftRear < 17.5)
            leftRear += rearIRat15;
        else
            leftRear += rearIRat20;
        //setLedDifference( leftFront - leftRear );
        debug("leftFront: " + leftFront + "leftRear: " + leftRear + "Sonar: " + front);
        leftRear = leftRear + 0.2f;
    }
    // 34 ticks 90 dec turn
    // 17 ticks 90 dec rotate
    // 1cm per tic
    private void followLeftNew(){
        updateIR();
        
        
        float front = sonarSensor.getReading();
        

        if( front <= TOOCLOSE ){
            debug( "Front too close turning right");
            motors.sendCommand(MotorController.ROTATE_RIGHT_N, 18);
            this.alignWithWall();
            motors.sendCommand(MotorController.STRAIGHT, CRUISESPEED);
            return;
        }
        
        if( leftFront <= CLOSE ){
            debug("Too close to wall turning away from wall");
            turnAwayFromWall( leftFront, leftRear );
            //alignWithWall();
            motors.sendCommand(MotorController.STRAIGHT, CRUISESPEED);
            return;
        }
       
        if( leftFront >= FAR ){
            if( leftFront - leftRear > 10 && front > TOOCLOSE ){
                debug("The front ir dropped off so I'm trying to turn around a left corner");
                motors.sendCommand(MotorController.FORWARD_N, 12);
                
                motors.sendCommand(MotorController.TURN_LEFT_N, 33);
                motors.sendCommand(MotorController.FORWARD_N, 14);
                //this.alignWithWall();
                motors.sendCommand(MotorController.STRAIGHT, CRUISESPEED);
                
                
                return;
            }

        }
    }
    
    private void setLedDifference( float difference ){
        int value = (int)( difference * 10 );
        value = Math.abs(value);
        int comp = 0x01;
        for( int i = 0; i < 8; i++ ){
            this.leds[i].setOn( ( value & comp ) == comp );
            comp = comp << 0x01;
        }
        
    }
    
    private void findIr(){
        if( !beaconSensor.detect()  ){
            // spin slowly until we find an IR reading
            int rotateCommand = getRotateCommand();
            while( !beaconSensor.detect() ){
                motors.sendCommand( rotateCommand, 30);
            }
            // found the IR keep spinning until we loose it
            int rotateCounter = 0;
            int rotateN;
            if( lastRotate == motors.ROTATE_LEFT )
                rotateN = motors.ROTATE_LEFT_N;
            else
                rotateN = motors.ROTATE_RIGHT_N;
            while( beaconSensor.detect() ){
                motors.sendCommand( rotateN, 1);
                rotateCounter++;
            }
            // lost the IR rotate back half what we rotated and go toards it
            if( lastRotate == motors.ROTATE_LEFT )
                rotateN = motors.ROTATE_RIGHT_N;
            else
                rotateN = motors.ROTATE_LEFT_N;
            motors.sendCommand( rotateN,(int)(rotateCounter/2) + 1);
        }
        
        //  go for IR until we loose it
        motors.sendCommand( motors.FORWARD, 40 );
     
    }
    
    int lastRotate = motors.ROTATE_LEFT;
    private int getRotateCommand(){
        if( lastRotate == motors.ROTATE_LEFT ){
            lastRotate = motors.ROTATE_RIGHT;
            return motors.ROTATE_RIGHT;
            
        }
        else{
            lastRotate = motors.ROTATE_LEFT;
            return motors.ROTATE_LEFT;
        }
    }

    
    private void alignWithWall(){
       debug("I'm aligning with wall");
       boolean aligned = false;
       int alignThreshold = 0;
       updateIR();
       float diff = leftFront - leftRear;
       if( diff >= 0 && diff <= 0.5 ) return;
       boolean rotatingRight = false;
       boolean rotatingLeft = false;
       
       while( !aligned ){
        updateIR();
        if( Math.abs( leftFront - leftRear ) > 0.1 ){
            if( leftFront > leftRear ){
                // if we were rotatingRight and now need to rotateLeft it is close enough so exit
                
                if( rotatingRight ){
                    alignThreshold++;
                    if( alignThreshold > 2 ){
                        aligned = true;
                        continue;
                    }
                }
                 
                // front is further out than rear
                // rotate left
                motors.sendCommand(MotorController.ROTATE_LEFT_N, 1);
                rotatingLeft = true;
           
            } else {
                
                if( rotatingLeft ){
                    alignThreshold++;
                    //if( alignThreshold > 2 ){
                        //aligned = true;
                        //continue;
                    //}
                }
                 
                // front is closer than rear
                // rotate right
                motors.sendCommand(MotorController.ROTATE_RIGHT_N, 1);
                rotatingRight = true;
            }
            toSleep(120);
        } else {
            aligned = true;
        }
       }
       
    }
    
    private void turnTowardWall( float front, float rear ){
      int ticks = 3;
      ticks += (int)(front - rear);
      motors.sendCommand(MotorController.ROTATE_LEFT_N, ticks);
      //motors.sendCommand(MotorController.FORWARD_N, 12);
      //motors.sendCommand(MotorController.ROTATE_RIGHT_N, ticks-1);
    }
    
    private void turnAwayFromWall( float front, float rear ){  
       int ticks = 3;
       ticks += (int)(rear - front);
       motors.sendCommand(MotorController.ROTATE_RIGHT_N, ticks); 
       //motors.sendCommand(MotorController.FORWARD_N, 12);
       //motors.sendCommand(MotorController.ROTATE_LEFT_N, ticks-1);  
    }
    
    // HELPER FUNCTIONS
    
    private float irLF(){
        return irSensors.getReading( LEFT_FRONT );
    }
    
    private float irLR(){
        return irSensors.getReading( LEFT_REAR );
    }
    
    private float irRF(){
        return irSensors.getReading( RIGHT_FRONT );
    }
    
    private float irRR(){
        return irSensors.getReading( RIGHT_REAR );
    }
    

    
    private void toSleep( int msec ){
         try {
                Thread.sleep(msec);
            } catch (InterruptedException e) {
                //We've been interrupted: exit the thread
                return;
            }
        
    }
    
    // TESTS BELOW
    
    private void testMotorController(){
        //int speed = 20;
        motors.sendCommand( MotorController.FORWARD, CRUISESPEED );
        toSleep(2000);
        motors.sendCommand( MotorController.REVERSE, CRUISESPEED );
        toSleep(2000);
        motors.sendCommand( MotorController.TURN_LEFT, CRUISESPEED );
        toSleep(2000);
        motors.sendCommand( MotorController.TURN_RIGHT, CRUISESPEED );
        toSleep(2000);
       
    }
    
    private void testIr( ){
        //int reading = ((IrSensor)irSensors.get( name )).getReading();
        System.out.println( "LEFT_FRONT: " + irSensors.getReading( LEFT_FRONT ) + "cm" + " LEFT_REAR: " + irSensors.getReading( LEFT_REAR ) + "cm" + " RIGHT_FRONT: " + irSensors.getReading( RIGHT_FRONT ) + "cm" + " RIGHT_REAR: " + irSensors.getReading( RIGHT_REAR ) + "cm");
        //System.out.println( "RIGHT_FRONT: " + irSensors.getReading( RIGHT_FRONT ) + "cm" + " RIGHT_REAR: " + irSensors.getReading( RIGHT_REAR ) + "cm");
        //System.out.println( " LEFT_REAR: " + irSensors.getReading( LEFT_REAR ) + "cm");
        //toSleep(2000);
    }
    
    private void testSonarServo( ){
        float test = 0.5f;
        for( float f = 0.0f; f <= 1.0f; f += 0.2f){
           sonarServo.setOrientation(test);
            System.out.println("SonarServo frontCenter moved to: " + test );
            toSleep(2000);
        }
    }
    
    private void testSonarReading( ){
        //for( float f = 0.0f; f <= 1.0f; f += 0.2f){
            float reading = sonarSensor.getReading();
            System.out.println("Sonar frontCenter: " + reading + "cm");
            //toSleep(2000);
        //}
    }
    
        
    

}
